#include "stdafx.h"
#include "MarchingCubesTSDF.h"





//template <typename PointNT>
//MarchingCubesTSDF<PointNT>::~MarchingCubesTSDF()
//{
//}

template <typename PointNT>
void MarchingCubesTSDF<PointNT>::SetSize(int x, int y, int z)
{
	m_isInit = false;
	setGridResolution(x, y, z);
	m_TSDFData = std::vector<suTSDFData>(res_x_*res_y_*res_z_);
}


template <typename PointNT>
void MarchingCubesTSDF<PointNT>::InsertData(PointCloudPtr pData, const vector<int>& vDepth2D, int iDataWidth, int iDataHeight, Eigen::Matrix4Xf matTranformKinect)
{
	if (!m_isInit)
	{
		//m_TSDFData = std::vector<suTSDFData>(res_x_*res_y_*res_z_);
		GetAABBBox(pData, m_PntMin, m_pntMax);
		m_isInit = true;
	}
	shared_ptr<KinectDataCollection> pKinect = KinectDataCollection::GetInstance();

	for (int ix = 0; ix < res_x_; ++ix)
	{
		const int y_start = ix * res_y_ * res_z_;

		for (int iy =0; iy<res_y_; ++iy)
		{
			const int z_start = y_start + iy * res_z_;


			vector<CameraSpacePoint> vCamPnt;
			vector<DepthSpacePoint> vDepthPnt;
			{
				PointCloudPtr pPoint = PointCloudPtr(new pcl::PointCloud<PointNT>());
				PointCloudPtr pOut = PointCloudPtr(new pcl::PointCloud<PointNT>());
				pPoint->resize(res_z_);
				for (int iz = 0; iz < res_z_; ++iz)
				{
					pPoint->points[iz].x = m_PntMin.x + GetXDis()*ix;
					pPoint->points[iz].y = m_PntMin.y + GetYDis()*iy;
					pPoint->points[iz].z = m_PntMin.z + GetZDis()*iz;
				}
				pcl::transformPointCloud(*pPoint, *pOut, matTranformKinect);

				vCamPnt.resize(res_z_);
				for (int iz = 0; iz < res_z_; ++iz)
				{
					vCamPnt[iz].X = (*pOut)[iz].x;
					vCamPnt[iz].Y = (*pOut)[iz].y;
					vCamPnt[iz].Z = (*pOut)[iz].z;
					//vCamPnt[iz].Z = 0;
					
				}

				pKinect->CameraToDepth(vCamPnt, vDepthPnt);
			}

			for (int iz = 0; iz < res_z_; ++iz)
			{
				CameraSpacePoint pntInKinect;
				float fDepthValue = CalDepthValue(vDepth2D, iDataWidth, iDataHeight, vDepthPnt[iz].X, vDepthPnt[iz].Y);
				if (fDepthValue > 0)
				{
					pKinect->DepthToCamera(vDepthPnt[iz], (int)(fDepthValue + 0.5), pntInKinect);
					m_TSDFData[z_start + iz].UpdateValue(vCamPnt[iz].Z - pntInKinect.Z);
				}
			}
		}
	}

}


template <typename PointNT>
void MarchingCubesTSDF<PointNT>::voxelizeData()
{
	for (int i = 0; i < m_TSDFData.size(); ++i)
	{
		grid_[i] = m_TSDFData[i].m_tsdfAvg;

	}
}

template <typename PointNT>
float  MarchingCubesTSDF<PointNT>::CalDepthValue(const vector<int>& vDepth2D, int iDataWidth, int iDataHeight, float fx, float fy)
{
	vector<pair<int, int>> vRegion;
	int x = (int)fx, y = (int)fy;
	if (x >= 0 && x < iDataWidth && y >= 0 && y < iDataHeight)
	{
		int xMinux = std::max(x - 1, 0), yMinux = std::max(y - 1, 0), xAdd = std::min(x + 1, iDataWidth - 1), yAdd = std::min(y + 1, iDataHeight - 1);
		if (x > 0 && y > 0)vRegion.push_back(make_pair(xMinux, yMinux));
		if (x > 0)vRegion.push_back(make_pair(xMinux, y));
		if (x > 0 && y != iDataHeight - 1)vRegion.push_back(make_pair(xMinux, yAdd));

		if (y > 0) vRegion.push_back(make_pair(x, yMinux));
		if (y != iDataHeight - 1)vRegion.push_back(make_pair(x, yAdd));

		if (x != iDataWidth - 1 && y > 0)vRegion.push_back(make_pair(xAdd, yMinux));
		if (x != iDataWidth - 1)vRegion.push_back(make_pair(xAdd, y));
		if (x != iDataWidth - 1 && y != iDataHeight - 1)vRegion.push_back(make_pair(xAdd, yAdd));
	}

	float ret = -1;
	int iNum = 0;
	for (const pair<int, int>& data: vRegion)
	{
		int inx = data.first + data.second*iDataWidth;
		if (vDepth2D[inx] >= 0)
		{
			if(ret < 0) ret= vDepth2D[inx];
			else	ret += vDepth2D[inx];
			++iNum;
		}
	}
	if (iNum > 0) ret /= iNum;
	return ret;
}

template <typename PointNT>
void MarchingCubesTSDF<PointNT>::GetAABBBox(const PointCloudPtr& pSrc, PointNT& pntMin, PointNT& pntMax)
{
	if (pSrc->size() > 0)
	{
		pntMin = pntMax = pSrc->points[0];
		for (PointNT pnt : pSrc->points)
		{
			pntMin.x = min(pnt.x, pntMin.x);
			pntMin.y = min(pnt.y, pntMin.y);
			pntMin.z = min(pnt.z, pntMin.z);

			pntMax.x = max(pnt.x, pntMax.x);
			pntMax.y = max(pnt.y, pntMax.y);
			pntMax.z = max(pnt.z, pntMax.z);
		}
	}
}

